/**
 * 
 */
package program;

import java.util.Arrays;
import java.util.HashMap;
import java.util.HashSet;
import java.util.Map;

import javax.vecmath.Point2d;

/**
 * Simple map
 * 
 * @version 2.0
 * @date May 2, 2012
 */
public class RoboMap {

	/** all map position is rounded to integers */
	public final int DEFUALT_ACCURACY = 1;

	/** Describe different types of terrain */
	public static enum Content {
		/** terrain is not yet set to be anything */
		UNKNOWN,
		/** terrain is set to be free and no moving is on the spot */
		CLEAR,
		/** terrain is occupied with either moving or non-moving obstacle */
		OBJECT,
		
		MAYBEOBJECT
	};

	/*** Size of internal data structure block */
	private final int SIZE = 20;
	/** How accurate the map is. 10 = one decimal, 100 = two decimals */
	private int accuracy = DEFUALT_ACCURACY;
	private Map<Point2d, Content[][]> map;
	private HashSet<MovingObject> movingObjects;
	
	private int assumeConcaveRange = DEFAULT_CONCAVERANGE;
	private boolean assumeConcave = DEFAULT_ASSUMECONCAVE;
	
	private static int DEFAULT_CONCAVERANGE = 2;
	private static boolean DEFAULT_ASSUMECONCAVE = false;

	/**
	 * Create map using DEFAULT_ACCURACY as accuracy
	 */
	public RoboMap() {
		super();
		map = new HashMap<Point2d, Content[][]>();
		movingObjects = new HashSet<MovingObject>();
	}

	/**
	 * Create map
	 * 
	 * @param accuracy
	 *            of map
	 */
	public RoboMap(int accuracy) {
		this();
		this.accuracy = accuracy;
	}
	
	public RoboMap(int accuracy, int assumeConcaveRange, boolean assumeConcave) {
		this(accuracy);
		this.assumeConcave = assumeConcave;
		this.assumeConcaveRange = assumeConcaveRange;
	}
	
	public RoboMap(int assumeConcaveRange, boolean assumeConcave) {
		this();
		this.assumeConcave = assumeConcave;
		this.assumeConcaveRange = assumeConcaveRange;
	}

	/**
	 * Change content on position in environment
	 * 
	 * @param position
	 *            , can be any number but will be rounded down to integer
	 * @param content
	 */
	public void setPosition(Point2d position, Content content) {
		Point2d blockPos = getBlockPos(position);
		Point2d posInBlock = getPosInBlock(position);

		Content[][] b = map.get(blockPos);
		if (b == null) {
			b = new Content[SIZE][SIZE];
			for (int i = 0; i < b.length; i++) {
				Arrays.fill(b[i], Content.UNKNOWN);
			}
			map.put(blockPos, b);
		}

		b[(int) posInBlock.x][(int) posInBlock.y] = content;
		
		if(assumeConcave) {
			assumeConcave = false;
			boolean up = false;
			boolean down = false;
			boolean right = false;
			boolean left = false;
			for(int i = assumeConcaveRange+1; i >= 1; i--) {
				
				if(this.getContentFromPosition(new Point2d(position.x+(i*accuracy),position.y)) == Content.OBJECT) {
					for(int j = i-1; j >= 1; j--) {
						Point2d current = new Point2d(position.x+(j*accuracy),position.y);
						if(getContentFromPosition(current) != Content.OBJECT) {
							setPosition(current, Content.MAYBEOBJECT);
						}
					}
					right = true;
				}
				if(this.getContentFromPosition(new Point2d(position.x-(i*accuracy),position.y)) == Content.OBJECT) {
					for(int j = i-1; j >= 1; j--) {
						Point2d current = new Point2d(position.x-(j*accuracy),position.y);
						if(getContentFromPosition(current) != Content.OBJECT) {
							setPosition(current, Content.MAYBEOBJECT);
						}
					}
					left = true;
				}
				if(this.getContentFromPosition(new Point2d(position.x,position.y+(i*accuracy))) == Content.OBJECT) {
					
					for(int j = i-1; j >= 1; j--) {
						Point2d current = new Point2d(position.x,position.y+(j*accuracy));
						if(getContentFromPosition(current) != Content.OBJECT) {
							setPosition(current, Content.MAYBEOBJECT);
						}
					}
					up = true;
				}
				if(this.getContentFromPosition(new Point2d(position.x,position.y-(i*accuracy))) == Content.MAYBEOBJECT) {
					for(int j = i-1; j >= 1; j--) {
						Point2d current = new Point2d(position.x,position.y-(j*accuracy));
						if(getContentFromPosition(current) != Content.OBJECT) {
							setPosition(current, Content.MAYBEOBJECT);
						}
					}
					down = true;
				}
				if(up && down && right && left) {
					break;
				}
				
			}
			assumeConcave = true;
		}

	}

	/**
	 * Add moving object to environment
	 * 
	 * @param position
	 *            center of the moving object.
	 * @param direction
	 *            Its direction
	 * @param time
	 *            when was it observed
	 */
	public void addMovingObject(Point2d position, Point2d direction, double time) {
		movingObjects.add(new MovingObject(position, direction, time));
	}

	/***
	 * Get left top corner of block containing position
	 * 
	 * @param position
	 * @return left top corner of block
	 */
	private Point2d getBlockPos(Point2d position) {
		//Rounding
		double x = (int) (position.x * accuracy + 0.5),
				y = (int) (position.y * accuracy + 0.5);
		
		if (x < 0) {
			x -= SIZE;
		}
		if (y < 0) {
			y -= SIZE;
		}
		
		//Cut decimals
		x /= SIZE;
		y /= SIZE;

		return new Point2d(SIZE * x, SIZE * y);
	}

	/***
	 * Get position with in block
	 * 
	 * @param position
	 * @return, position will be between 0 - SIZE
	 */
	private Point2d getPosInBlock(Point2d position) {
		//Rounding				
		double x = (int) (position.x * accuracy + (position.x >= 0 ? 0.5 : -0.5))
				% SIZE; 
		double y = (int) (position.y * accuracy + (position.y >= 0 ? 0.5 : -0.5))
				% SIZE;

		if (x < 0) {
			x += SIZE;
		}
		if (y < 0) {
			y += SIZE;
		}
		return new Point2d(x, y);
	}
	
	/**
	 * Check if whether a content is in a path
	 * @param c
	 * @param path
	 * @return
	 */
	public boolean contain (Content c, Point2d[] path) {
		for (Point2d p : path) {
			if (getContentFromPosition(p) == c) {
				return true;
			}
		}
		return false;
	}
	
	/**
	 * Check if a certain point contains a certain object.
	 * @param c
	 * @param point
	 * @return if it contains then true
	 */
	public boolean contain(Content c, Point2d point) {
		return getContentFromPosition(point) == c;
	}

	/**
	 * Check if a certain point contains a certain object at time
	 * @param c
	 * @param point
	 * @param time
	 * @return
	 */
	public boolean contain (Content c, Point2d point, double time) {
		return c == getContentFromPosition(point, time);
	}
	
	@Override
	public String toString() {
		return "Number of blocks: " + map.size()
				+ "\nNumber of moving objects: " + movingObjects.size();
	}

	/**
	 * Return content on position
	 * 
	 * @param position
	 *            , can be any number
	 * @return
	 */
	public Content getContentFromPosition(Point2d position) {
		Point2d blockPos = getBlockPos(position);
		Point2d posInBlock = getPosInBlock(position);

		Content[][] b = map.get(blockPos);
		if (b == null) {
			return Content.UNKNOWN;
		}
		return b[(int) posInBlock.x][(int) posInBlock.y];

	}

	/**
	 * Returns content on position and take for account moving objects
	 * 
	 * @param position
	 *            , can be any number
	 * @param t
	 *            when is this position interesting
	 * @return OBJECT for both moving and non-moving objects.
	 */
	public Content getContentFromPosition(Point2d position, double t) {
		Content atPos = getContentFromPosition(position);
		if (atPos == Content.OBJECT) {
			return Content.OBJECT;
		}

		for (MovingObject mo : movingObjects) {
			if (mo.isCollision(position, t)) {
				return Content.OBJECT;
			}
		}
		return atPos;
	}

	/**
	 * Get where c is in map in interval min and max
	 * @param c
	 * @param min
	 * @param max
	 * @return
	 */
	public String getMap (Content c, int min, int max) {
		StringBuilder sb = new StringBuilder();
		sb.append(this.toString() + "\n");
		
		for (int i = min; i <= max ; i++) {
			for (int j = min; j <= max; j++) {
				Content atPos = getContentFromPosition(new Point2d(i,j));
				if (atPos == c) {
					sb.append(c + "(" + i + "," + j + ")\n");
				}
			}
		}
		
		return sb.toString();
	}

	/**
	 * Get where all moving objects are and where they are going.
	 * @return
	 */
	public String getMovingObjects() {
		StringBuilder sb = new StringBuilder();
		for (MovingObject mo : movingObjects) {
			sb.append(mo + "\n");
		}
		return sb.toString();
	}

	/**
	 * Used to describe moving object in environment
	 * 
	 * @date 6 maj 2012
	 * @version 1.0
	 */
	private class MovingObject {
		/**
		 * Minimum distance between to object before they are considered to have
		 * collide
		 */
		public static final int COLLISION_DISTANCE = 1;

		/*** Position at time <i>time</i> */
		private Point2d position;
		/*** Direction at time <i>time</i> */
		private Point2d direction;
		private double time;

		public MovingObject(Point2d position, Point2d direction, double time) {
			this.position = position;
			this.direction = direction;
			this.time = time;
		}

		public boolean isCollision(Point2d position, double time) {
			double dTime = time - this.time;
			double x = this.position.x + this.direction.x * dTime, y = this.position.y
					+ this.direction.y * dTime;
			double distance = new Point2d(x, y).distance(position);
			return distance < COLLISION_DISTANCE;
		}
		
		@Override
		public int hashCode () {
			int sum = (int) (position.hashCode() + (direction.x + direction.y) * -time);
			return sum;
		}
		
		@Override
		public boolean equals (Object o) {
			if (this == o) {
				return true;
			}
			
			MovingObject mo = (MovingObject) o;
			double dTime = time - mo.time;
			Point2d dPos = new Point2d (position.x + direction.x * dTime,
							position.y + direction.y * dTime);
			return dPos.equals(mo.position);
		}
		
		@Override
		public String toString () {
			return "Position: " + position
					+ "\nDirection: " + direction;
		}
	}
	
	
}
